﻿using MathNet.Numerics.LinearAlgebra;
using System.Data;
using System.Numerics;
using Matrix = MathNet.Numerics.LinearAlgebra.Double.DenseMatrix;
using Vector = MathNet.Numerics.LinearAlgebra.Double.DenseVector;

namespace PBDsim;
public class TetrahedronMesh
{
    List<(int index, Vector qc, Vector nc)> CollisionConstrains = [];
    List<int> FaceIndices = [];
    List<Face> Faces = [];
    List<int> FixedConstrains = [];
    int[] Indices;
    int nTet;
    int nVert;
    public double poissonRatio = 0.3;
    public double youngsModulus = 72E9;
    double mu;
    double lm;
    List<double> SurfaceNorm = [];
    List<int> SurfaceVertices = [];
    Tet[] Tets;
    Vertex[] Vertices;
    List<int>[] VerticesEdges;
    List<uint>[] VerticesFaces;
    List<int>[] VerticesTets;
    List<XpbdFemTetConstraint> XpbdFemTetConstraints = [];
    Matrix I = Matrix.CreateIdentity(3);

    public TetrahedronMesh(Vertex[] vertices, int[] indices)
    {
        Vertices = vertices;
        Indices = indices;
        nTet = Indices.Length / 4;
        nVert = Vertices.Length;
        VerticesFaces = new List<uint>[nVert];
        VerticesTets = new List<int>[nVert];
        VerticesEdges = new List<int>[nVert];
        Tets = new Tet[nTet];
        // 柔度系数定义为1/杨氏模量，此处单位化 ——git pbd
        mu = youngsModulus / 2 / (1.0 + poissonRatio) / youngsModulus;
        lm = youngsModulus * poissonRatio / (1.0 + poissonRatio) / (1.0 - 2.0 * poissonRatio) / youngsModulus;
        Init();
    }
    private void ProjectContrains(Vector GlobalPosition, double dt)
    {
        double alpha = 1.0 / (youngsModulus * dt * dt);
        bool satisfy;
        int inner_iter = 0;
        double multiplier = 0;
        var COLLISION_EPS = 0.1;
        do
        {
            if (inner_iter == 0)
                multiplier = 0.0;
            inner_iter++;
            satisfy = true;
            for (int icons = 0; icons < XpbdFemTetConstraints.Count; icons++)
            {
                var constrain = XpbdFemTetConstraints[icons];
                bool handleInversion = false;
                var iTet = constrain.iTet;
                var invRestMat = Tets[iTet].InvRestMatrix;
                var restVolumn = Tets[iTet].RestVolumn;
                var currentVolumn = CalcTetVolumn(iTet);
                if (currentVolumn / restVolumn < 0.2) handleInversion = true;

                var x1 = Vertices[Indices[iTet * 4 + 0]].p;
                var x2 = Vertices[Indices[iTet * 4 + 1]].p;
                var x3 = Vertices[Indices[iTet * 4 + 2]].p;
                var x4 = Vertices[Indices[iTet * 4 + 3]].p;

                var Ds = Matrix.OfColumnVectors(x1 - x4, x2 - x4, x3 - x4);
                var Dm_1 = invRestMat!;
                var F = Ds * Dm_1;

                Matrix<double> E, P;
                double tr_E;
                if (!handleInversion || currentVolumn > 0.0)
                {
                    //格林应变
                    var F_T = F.Transpose();
                    E = 1.0 / 2 * (F_T * F - I);

                    //PK1应力 P   
                    tr_E = E.Trace();
                    P = F * (2 * mu * E + lm * tr_E * I);
                }
                else
                {
                    var (hatF, U, VT) = Extend.SvdWithInversionHandling(F);
                    double minXVal = 0.577;
                    for (int j = 0; j < 3; j++)
                    {
                        if (hatF[j] < minXVal)
                            hatF[j] = minXVal;
                    }

                    // epsilon for hatF
                    Vector epsilonHatF = Vector.OfArray(
                        [0.5 *(hatF[0] * hatF[0] - 1),
                        0.5 * (hatF[1] * hatF[1] - 1),
                        0.5 * (hatF[2] * hatF[2] - 1)]);

                    double trace = epsilonHatF[0] + epsilonHatF[1] + epsilonHatF[2];
                    tr_E = trace;
                    double ltrace = lm * trace;
                    Vector sigmaVec = epsilonHatF * 2.0 * mu;
                    sigmaVec[0] += ltrace;
                    sigmaVec[1] += ltrace;
                    sigmaVec[2] += ltrace;
                    sigmaVec[0] = hatF[0] * sigmaVec[0];
                    sigmaVec[1] = hatF[1] * sigmaVec[1];
                    sigmaVec[2] = hatF[2] * sigmaVec[2];

                    var sigmaDiag = new Matrix(3);
                    var epsDiag = new Matrix(3);
                    sigmaDiag.SetRow(0, Vector.OfArray([sigmaVec[0], 0.0, 0.0]));
                    sigmaDiag.SetRow(1, Vector.OfArray([0.0, sigmaVec[1], 0.0]));
                    sigmaDiag.SetRow(2, Vector.OfArray([0.0, 0.0, sigmaVec[2]]));

                    epsDiag.SetRow(0, Vector.OfArray([epsilonHatF[0], 0.0, 0.0]));
                    epsDiag.SetRow(1, Vector.OfArray([0.0, epsilonHatF[1], 0.0]));
                    epsDiag.SetRow(2, Vector.OfArray([0.0, 0.0, epsilonHatF[2]]));

                    E = U * epsDiag * VT;
                    P = U * sigmaDiag * VT;
                }
                //应变能密度
                var Psi = mu * (E * E).Trace() + lm / 2.0 * (tr_E * tr_E);

                //应变能
                var energy = Psi * restVolumn;

                //节点受力
                var H = restVolumn * P * Dm_1.Transpose();
                var f1 = H.Column(0);
                var f2 = H.Column(1);
                var f3 = H.Column(2);
                var f4 = -f1 - f2 - f3;

                // By choosing the constraint function as sqrt(2 U'), the potential energy used in XPBD: 
                // U = 0.5 * alpha^-1 * C^2
                // gives us exactly the required potential energy of the elastic solid. 
                double U_ = energy;
                double C = Math.Sqrt(2.0 * U_);
                var gradU_ = new MathNet.Numerics.LinearAlgebra.Vector<double>[] { f1, f2, f3, f4 };


                //double sum_normGradU_ =
                //    invMass0 * gradU_[0].Norm(2) * gradU_[0].Norm(2) +
                //    invMass1 * gradU_[1].Norm(2) * gradU_[1].Norm(2) +
                //    invMass2 * gradU_[2].Norm(2) * gradU_[2].Norm(2) +
                //    invMass3 * gradU_[3].Norm(2) * gradU_[3].Norm(2);
                double sum_normGradU_ = gradU_.Select(
                    (gradU, i) => Vertices[Indices[iTet * 4 + i]].InverseMass * gradU.Norm(2) * gradU.Norm(2))
                    .Sum();

                // Note that grad C = 1/C grad U', by multiplying C^2 to alpha, we now can add the factor C^2 to the nominator
                sum_normGradU_ += C * C * alpha;

                if (sum_normGradU_ < 1E-10) continue;

                // compute scaling factor
                var lmbda = -C * (C + alpha * multiplier) / sum_normGradU_;     // since in the next step we use gradU instead of gradC, we only add the factor C instead of C^2
                multiplier += lmbda;

                for (int i = 0; i < 4; i++)
                {
                    var invMass = Vertices[Indices[iTet * 4 + i]].InverseMass;
                    if (invMass != 0)
                    {
                        var corr = (Vector)(lmbda * invMass * gradU_[i]);
                        Vertices[Indices[iTet * 4 + i]].p += corr;

                    }

                }
            }
            foreach (var collision in CollisionConstrains)
            {
                var index = collision.index;
                var qc = collision.qc;
                var nc = collision.nc;
                var pi = Vertices[index].p + GlobalPosition;
                var Cp = (pi - qc).DotProduct(nc);
                if (Cp > 0) continue;
                else satisfy = false;
                var GradCp = nc;
                var delta_p = -(Cp * GradCp / (GradCp.L2Norm() * GradCp.L2Norm())) + (float)COLLISION_EPS * GradCp;
                if (delta_p.L2Norm() > COLLISION_EPS)
                {
                    satisfy = false;
                }
                Vertices[index].p += delta_p;
            }
            //if (iteration > 1) satisfy = true;
        } while (!satisfy);
    }

    public static TetrahedronMesh MakeTetrahedron(int width, int height, int depth, Vector translation, Matrix rotation, Vector scale)
    {
        Vector[] points = new Vector[width * height * depth];
        double dx = scale[0] / (width - 1);
        double dy = scale[1] / (height - 1);
        double dz = scale[2] / (depth - 1);

        var t = translation - 0.5 * scale;
        for (int i = 0; i < width; i++)
        {
            for (int j = 0; j < height; j++)
            {
                for (int k = 0; k < depth; k++)
                {
                    var x = dx * i;
                    var y = dy * j;
                    var z = dz * k;
                    points[i * height * depth + j * depth + k] = rotation * (Vector.OfArray([x, y, z])) + t;
                }
            }
        }

        List<int> indices = [];
        for (int i = 0; i < width - 1; i++)
        {
            for (int j = 0; j < height - 1; j++)
            {
                for (int k = 0; k < depth - 1; k++)
                {
                    // For each block, the 8 corners are numerated as:
                    //     4*-----*7
                    //     /|    /|
                    //    / |   / |
                    //  5*-----*6 |
                    //   | 0*--|--*3
                    //   | /   | /
                    //   |/    |/
                    //  1*-----*2
                    int p0 = i * height * depth + j * depth + k;
                    int p1 = p0 + 1;
                    int p3 = (i + 1) * height * depth + j * depth + k;
                    int p2 = p3 + 1;
                    int p7 = (i + 1) * height * depth + (j + 1) * depth + k;
                    int p6 = p7 + 1;
                    int p4 = i * height * depth + (j + 1) * depth + k;
                    int p5 = p4 + 1;

                    // Ensure that neighboring tetras are sharing faces
                    if ((i + j + k) % 2 == 1)
                    {
                        indices.Add(p2); indices.Add(p1); indices.Add(p6); indices.Add(p3);
                        indices.Add(p6); indices.Add(p3); indices.Add(p4); indices.Add(p7);
                        indices.Add(p4); indices.Add(p1); indices.Add(p6); indices.Add(p5);
                        indices.Add(p3); indices.Add(p1); indices.Add(p4); indices.Add(p0);
                        indices.Add(p6); indices.Add(p1); indices.Add(p4); indices.Add(p3);
                    }
                    else
                    {
                        indices.Add(p0); indices.Add(p2); indices.Add(p5); indices.Add(p1);
                        indices.Add(p7); indices.Add(p2); indices.Add(p0); indices.Add(p3);
                        indices.Add(p5); indices.Add(p2); indices.Add(p7); indices.Add(p6);
                        indices.Add(p7); indices.Add(p0); indices.Add(p5); indices.Add(p4);
                        indices.Add(p0); indices.Add(p2); indices.Add(p7); indices.Add(p5);
                    }
                }
            }
        } // end for loop

        var Vertices = points.Select(i => new Vertex(i)).ToArray();

        return new TetrahedronMesh(Vertices, [.. indices]);
    }
    public void Init()
    {
        BuildNeighbors();
        BuildNorm();
        LumpedMass(totalMass: 100);
        MakeConstrains();
    }
    public void BuildNeighbors()
    {
        for (int i = 0; i < nVert; i++)
        {
            VerticesTets[i] = [];
            VerticesFaces[i] = [];
            VerticesEdges[i] = [];
        }
        for (int i = 0; i < nTet; i++)
        {
            var vol = CalcTetVolumn(i);
            if (vol < 0) throw new Exception("volumn < 0");
            Tets[i] = new Tet
            {
                RestVolumn = vol
            };
        }
        List<Edge> Edges = [];
        for (int i = 0; i < nTet; i++)
        {
            // tet edge indices: {0,1, 0,2, 0,3, 1,2, 1,3, 2,3} // 的
            int[] edges = [
                Indices[4*i], Indices[4*i+1],
                Indices[4*i], Indices[4*i+2],
                Indices[4*i], Indices[4*i+3],
                Indices[4*i+1], Indices[4*i+2],
                Indices[4*i+1], Indices[4*i+3],
                Indices[4*i+2], Indices[4*i+3]];

            // tet face indices: {0,1,2, 1,3,2, 3,0,2, 1,0,3} => clock wise
            /*const unsigned int faces[12] = {	Indices[4*i], Indices[4*i+1], Indices[4*i+2], 
                                                Indices[4*i+1], Indices[4*i+3], Indices[4*i+2], 
                                                Indices[4*i+3], Indices[4*i], Indices[4*i+2], 
                                                Indices[4*i+1], Indices[4*i], Indices[4*i+3]};*/

            // tet face indices: {1,0,2, 3,1,2, 0,3,2, 0,1,3} => counter clock wise
            int[] faces = [
                Indices[4*i+1], Indices[4*i], Indices[4*i+2],
                Indices[4*i+3], Indices[4*i+1], Indices[4*i+2],
                Indices[4*i], Indices[4*i+3], Indices[4*i+2],
                Indices[4*i], Indices[4*i+1], Indices[4*i+3]];

            for (int j = 0; j < 4; j++)
            {
                // add vertex-tet connection
                int vIndex = Indices[4 * i + j];
                // 一个顶点编号对应的四面体编号
                VerticesTets[vIndex].Add(i);
            }

            for (int j = 0; j < 4; j++)
            {
                // add face information
                int a = faces[j * 3 + 0];
                int b = faces[j * 3 + 1];
                int c = faces[j * 3 + 2];
                uint faceId = 0xffffffff;
                // find face
                for (int k = 0; k < VerticesFaces[a].Count; k++)
                {
                    // Check if we already have this face in the list
                    int faceIndex = (int)VerticesFaces[a][k];
                    if (((FaceIndices[3 * faceIndex] == a) || (FaceIndices[3 * faceIndex] == b) || (FaceIndices[3 * faceIndex] == c)) &&
                        ((FaceIndices[3 * faceIndex + 1] == a) || (FaceIndices[3 * faceIndex + 1] == b) || (FaceIndices[3 * faceIndex + 1] == c)) &&
                        ((FaceIndices[3 * faceIndex + 2] == a) || (FaceIndices[3 * faceIndex + 2] == b) || (FaceIndices[3 * faceIndex + 2] == c)))
                    {
                        faceId = VerticesFaces[a][k];
                        break;
                    }
                }
                if (faceId == 0xffffffff)
                {
                    // create new
                    Face f = new();
                    FaceIndices.Add(a);
                    FaceIndices.Add(b);
                    FaceIndices.Add(c);
                    faceId = (uint)(FaceIndices.Count / 3 - 1);
                    f.Tets[0] = (uint)i;
                    f.Tets[1] = 0xffffffff;
                    Faces.Add(f);

                    // add vertex-face connection				
                    VerticesFaces[a].Add(faceId);
                    VerticesFaces[b].Add(faceId);
                    VerticesFaces[c].Add(faceId);
                }
                else
                {
                    Face fd = Faces[(int)faceId];
                    fd.Tets[1] = (uint)i;
                }
                // append face
                Tets[i].Faces[j] = (int)faceId;
            }

            for (int j = 0; j < 6; j++)
            {
                // add face information
                int a = edges[j * 2 + 0];
                int b = edges[j * 2 + 1];
                uint edge = 0xffffffff;
                // find edge
                for (int k = 0; k < VerticesEdges[a].Count(); k++)
                {
                    // Check if we already have this edge in the list
                    Edge e = Edges[VerticesEdges[a][k]];
                    if (((e.Verts[0] == a) || (e.Verts[0] == b)) &&
                        ((e.Verts[1] == a) || (e.Verts[1] == b)))
                    {
                        edge = (uint)VerticesEdges[a][k];
                        break;
                    }
                }
                if (edge == 0xffffffff)
                {
                    // create new
                    Edge e = new();
                    e.Verts[0] = a;
                    e.Verts[1] = b;
                    Edges.Add(e);
                    edge = (uint)Edges.Count - 1;

                    // add vertex-edge connection				
                    VerticesEdges[a].Add((int)edge);
                    VerticesEdges[b].Add((int)edge);
                }
                // append edge
                Tets[i].Edges[j] = (int)edge;
            }
        }

    } // End BuildNeighbors

    public void BuildNorm()
    {
        for (int i = 0; i < Faces.Count; i++)
        {
            if ((Faces[i].Tets[0] == 0xffffffff) || (Faces[i].Tets[1] == 0xffffffff))
            {
                SurfaceVertices.Add(FaceIndices[i * 3 + 0]);
                SurfaceVertices.Add(FaceIndices[i * 3 + 1]);
                SurfaceVertices.Add(FaceIndices[i * 3 + 2]);
            }
        }

    }
    public void UpdateNorm()
    {
        SurfaceNorm.Clear();
        for (int i = 0; i < SurfaceVertices.Count; i += 3)
        {
            var a = Vertices[SurfaceVertices[i + 0]].x;
            var b = Vertices[SurfaceVertices[i + 1]].x;
            var c = Vertices[SurfaceVertices[i + 2]].x;
            var v1 = b - a;
            var v2 = c - a;
            var norm = v1.Cross(v2).Normalize(2);
            SurfaceNorm.Add(norm[0]);
            SurfaceNorm.Add(norm[1]);
            SurfaceNorm.Add(norm[2]);
        }
        for (int i = 0; i < SurfaceNorm.Count; i += 3)
        {
            var sv1 = SurfaceVertices[i + 0];
            var sv2 = SurfaceVertices[i + 1];
            var sv3 = SurfaceVertices[i + 2];
            var VertexNorm = new Vector([SurfaceNorm[i], SurfaceNorm[i + 1], SurfaceNorm[i + 2]]);
            Vertices[sv1].Norm += VertexNorm;
            Vertices[sv2].Norm += VertexNorm;
            Vertices[sv3].Norm += VertexNorm;
        }
        for (int i = 0; i < Vertices.Length; i++)
        {
            if (Vertices[i].Norm.Norm(2) < 1E-5)
            {
                Vertices[i].Norm = (Vector)Vector.OfArray([1, 1, 1]).Normalize(2);
            }
            else
            {
                Vertices[i].Norm = (Vector)(Vertices[i].Norm.Normalize(2));
            }
        }
    }
    public double CalcTetVolumn(int iTet)
    {
        var a = Vertices[Indices[iTet * 4 + 0]].x;
        var b = Vertices[Indices[iTet * 4 + 1]].x;
        var c = Vertices[Indices[iTet * 4 + 2]].x;
        var d = Vertices[Indices[iTet * 4 + 3]].x;
        var v1 = b - a;
        var v2 = c - a;
        var v3 = d - a;
        return Math.Abs(v1.Cross(v2).DotProduct(v3)) / 6.0f;
    }

    public void AddFixedConstrain(int i)
    {
        FixedConstrains.Add(i);
    }
    public void MakeConstrains()
    {
        var height = 5;
        var depth = 5;
        for (int i = 0; i < 1; i++)
        {
            for (int j = 0; j < height; j++)
            {
                for (int k = 0; k < depth; k++)
                {
                    AddFixedConstrain(i);
                }
            }
        }

        for (int i = 0; i < Tets.Length; i++)
        {
            var p0 = Vertices[Indices[i * 4 + 0]].x;
            var p1 = Vertices[Indices[i * 4 + 1]].x;
            var p2 = Vertices[Indices[i * 4 + 2]].x;
            var p3 = Vertices[Indices[i * 4 + 3]].x;
            var m1 = p0 - p3;
            var m2 = p1 - p3;
            var m3 = p2 - p3;
            var m = Matrix.OfColumnVectors(m1, m2, m3);
            var det = m.Determinant();
            if (Math.Abs(det) > 1E-6)
            {
                var invRestMat = m.Inverse();
                Tets[i].InvRestMatrix = (Matrix)invRestMat;

                XpbdFemTetConstraints.Add(new()
                {
                    iTet = i,
                });
            }
            else throw new Exception("Determinant of RestMat is almost Zero!");
        }
    }

    public void LumpedMass(double totalMass)
    {
        double totalVol = 0;
        // ρ = 1  m = v
        for (int i = 0; i < Tets.Length; i++)
        {
            totalVol += Tets[i].RestVolumn;
            var oneFourth = Tets[i].RestVolumn / 4.0; ;
            Vertices[Indices[i * 4 + 0]].Mass += oneFourth;
            Vertices[Indices[i * 4 + 1]].Mass += oneFourth;
            Vertices[Indices[i * 4 + 2]].Mass += oneFourth;
            Vertices[Indices[i * 4 + 3]].Mass += oneFourth;
        }
        // ρ = m / v  计算密度
        var rho = totalMass / totalVol;
        for (int i = 0; i < Vertices.Length; i++)
        {
            Vertices[i].Mass *= rho;
            Vertices[i].InverseMass = 1 / Vertices[i].Mass;
        }

    }
    public void Step(double dt, Vector GlobalPosition)
    {
        int iteration = 0;
        int outer_iter = 1000;
        var g = 9.8;
        while (iteration++ < outer_iter)
        {
            // 初始化位置和速度 已在节点初始化时完成
            // 以下开始循环
            //      更新节点速度  vi ← vi +Δt * wi * fext(xi)
            //      速度阻尼
            //      在无约束的情况下更新粒子位置：pi ← xi +Δt * vi
            //      碰撞约束：xi → pi
            //      迭代速度投影
            //      更新粒子速度和位置：vi ← (pi −xi)/Δt, xi ← pi
            for (int i = 0; i < Vertices.Length; i++)
            {
                if (FixedConstrains.Contains(i))
                    Vertices[i].InverseMass = 0;// InverMass在速度积分时使用
                var f_gravity = Vertices[i].Mass * Vector.OfArray([0, -1, 0]) * (float)g;
                var f_ext = Vector.OfArray([0, 0, 0]);
                Vertices[i].Force = f_gravity + f_ext;

                var damping = 0.99f;
                //半隐式积分
                // 更新节点速度
                Vertices[i].SemiImplicitEulerIntegrate(dt, damping);
                //碰撞检测
                //在Godot的MeshInstance3D里，Mesh的位置是相对于MeshInstance3D的位置的。
                //但是当我们使用PhysicsDirectSpaceState3D的IntersectRay方法时，
                //我们需要传入的是世界坐标系下的位置。
                //所以我们需要将Mesh的位置和顶点的位置相加，得到世界坐标系下的位置。
                //在构造Mesh时，使用的是Mesh的局部坐标系。
                //在更新Mesh位置时，可以认为Mesh具有平移不变性，在世界和局部坐标系下进行位置操作是一样的。
                //另外，MeshInstance3D放到场景中时，ArrayMesh必须放到子节点里，否则位置不正确，Z方向在不同编译模式下
                //具有不同的值，不知道为什么。
                if (Vertices[i].CollisionDetection(GlobalPosition, i, out var collision))
                {
                    CollisionConstrains.Add(collision!.Value);
                }

            }
            var vs = Vertices.Select(i => i.v).ToArray();
            var ds = Vertices.Select(i => i.x - i.x0).ToArray();
            for (int i = 0; i < ds.Length; i++)
            {
                Console.WriteLine(ds[i].ToVectorString());
            }
            // 迭代位置投影
            ProjectContrains(GlobalPosition, dt);
            for (int i = 0; i < Vertices.Length; i++)
            {
                // 更新粒子速度和位置
                Vertices[i].UpdateVelocity(dt);
            }
        }
        UpdateNorm();
    }
}

public class XpbdFemTetConstraint
{
    public int iTet;

}

public record struct Vertex
{
    public Vector x;
    public Vector x0;
    public Vector p;
    public Vector v;
    public Vector Force;
    public Vector Norm;
    public Vector Acceleration;
    public double Mass;
    public double InverseMass;
    public Vertex(Vector position)
    {
        var Zero = Vector.OfArray([0, 0, 0]);
        x0 = position;
        x = position;
        p = Zero;
        v = Zero;
        Norm = Zero;
        Force = Zero;
        Acceleration = Zero;
        Mass = 0.0f;
        InverseMass = 0.0f;
    }

    public void SemiImplicitEulerIntegrate(double delta, double damping)
    {
        v += delta * InverseMass * Force;
        //TODO 速度阻尼
        v *= damping;
        // 在无约束的情况下更新粒子位置
        p = x + v * delta;
    }
    public void UpdateVelocity(double delta)
    {
        v = (p - x) / delta;
        x = p;
    }
    public bool CollisionDetection(Vector GlobalPosition, int i, out (int index, Vector qc, Vector nc)? collision)
    {
        var xi = x + GlobalPosition;
        var pi = p + GlobalPosition;
        var sphere_center = Vector.OfArray([-40, 10, 26]);
        var radius = 10;
        var pi2c = pi - sphere_center;
        var xi2c = xi - sphere_center;
        // xi在球内，pi在球外，xi->pi与球相交
        if (pi2c.L2Norm() <= radius && xi2c.L2Norm() >= radius)
        {
            Extend.SphereLine(xi.ToVector3(), pi.ToVector3(), sphere_center.ToVector3(), radius, out var _, out var _, out var o1, out var _);
            var qc = o1!.Value;
            var nc = (Vector)(qc.ToDenseVector() - sphere_center).Normalize(2);
            collision = (i, qc.ToDenseVector(), nc);
            return true;
        }
        // xi->pi在球内
        else if (pi2c.L2Norm() <= radius && xi2c.L2Norm() <= radius)
        {
            var qs = (Vector)(sphere_center + pi2c.Normalize(2) * radius);
            var ns = (Vector)(qs - sphere_center).Normalize(2);
            collision = (i, qs, ns);
            return true;
        }
        collision = null;
        return false;
    }
}
public record Face
{
    // edge indices
    public int[] Edegs = [0, 0, 0];
    // tet indices
    public uint[] Tets = [0, 0];
};
public record Tet
{
    public int[] Edges = new int[6];
    public int[] Faces = new int[4];
    public double RestVolumn;
    public Matrix? InvRestMatrix;
    public double CurrentVolumn;
};
public record Edge
{
    public int[] Verts = [0, 0];
};
public static class Extend
{
    public static Vector Cross(this Vector left, Vector right)
    {
        if ((left.Count != 3 || right.Count != 3))
        {
            string message = "Vectors must have a length of 3.";
            throw new Exception(message);
        }
        Vector result = new(3)
        {
            [0] = left[1] * right[2] - left[2] * right[1],
            [1] = -left[0] * right[2] + left[2] * right[0],
            [2] = left[0] * right[1] - left[1] * right[0]
        };
        return result;
    }

    public static int SphereLine(Vector3 p1, Vector3 p2, Vector3 sc, double r, out double? mu1, out double? mu2, out Vector3? o1, out Vector3? o2)
    {
        double a, b, c;
        double bb4ac;
        Vector3 dp;
        dp.X = p2.X - p1.X;
        dp.Y = p2.Y - p1.Y;
        dp.Z = p2.Z - p1.Z;
        a = dp.X * dp.X + dp.Y * dp.Y + dp.Z * dp.Z;
        b = 2 * (dp.X * (p1.X - sc.X) + dp.Y * (p1.Y - sc.Y) + dp.Z * (p1.Z - sc.Z));
        c = sc.X * sc.X + sc.Y * sc.Y + sc.Z * sc.Z;
        c += p1.X * p1.X + p1.Y * p1.Y + p1.Z * p1.Z;
        c -= 2 * (sc.X * p1.X + sc.Y * p1.Y + sc.Z * p1.Z);
        c -= r * r;
        bb4ac = b * b - 4 * a * c;
        if (bb4ac < 0)
        {
            o1 = null;
            o2 = null;
            mu1 = null;
            mu2 = null;
            return 0;
        }
        else if (Math.Abs(a) == 0)
        {
            mu1 = -b / c;
            mu2 = null;
            o1 = p1 + (p2 - p1) * (float)mu1;
            o2 = null;
            return 1;
        }
        else
        {
            mu1 = (-b + Math.Sqrt(bb4ac)) / (2 * a);
            mu2 = (-b - Math.Sqrt(bb4ac)) / (2 * a);
            // 确保交点mu1在p1和p2之间
            if (mu2 >= 0 && mu2 <= 1) (mu1, mu2) = (mu2, mu1);
            o1 = p1 + (p2 - p1) * (float)mu1;
            o2 = p1 + (p2 - p1) * (float)mu2;
            return 2;
        }
    }
    public static Vector ToDenseVector(this Vector3 v)
    {
        return Vector.OfArray([v.X, v.Y, v.Z]);
    }

    public static Vector3 ToVector3(this Vector v)
    {
        return new Vector3((float)v[0], (float)v[1], (float)v[2]);
    }

    public static (Vector sigma, Matrix U, Matrix VT) SvdWithInversionHandling(this Matrix A)
    {
        Matrix U = Matrix.CreateIdentity(3);
        var sigma = Vector.OfArray([0, 0, 0]);
        var I = Matrix.CreateIdentity(3);
        var evd = (A.Transpose() * A).Evd();
        var S = evd.EigenValues;
        var V = evd.EigenVectors;
        var detV = V.Determinant();
        int pos = 0;
        if (detV < 0)
        {
            var minLambda = double.MaxValue;
            pos = 0;
            for (int l = 0; l < 3; l++)
            {
                if (S[l].Real < minLambda)
                {
                    pos = l;
                    minLambda = S[l].Real;
                }
                V[0, pos] = -V[0, pos];
                V[1, pos] = -V[1, pos];
                V[2, pos] = -V[2, pos];
            }
        }

        if (S[0].Real < 0.0) S[0] = 0.0;     // safety for sqrt
        if (S[1].Real < 0.0) S[1] = 0.0;
        if (S[2].Real < 0.0) S[2] = 0.0;

        sigma[0] = Math.Sqrt(S[0].Real);
        sigma[1] = Math.Sqrt(S[1].Real);
        sigma[2] = Math.Sqrt(S[2].Real);

        Matrix VT = (Matrix)V.Transpose();

        //
        // Check for values of hatF near zero
        //
        int chk = 0;
        pos = 0;
        for (int l = 0; l < 3; l++)
        {
            if (Math.Abs(sigma[l]) < 1.0e-4)
            {
                pos = l;
                chk++;
            }
        }

        if (chk > 0)
        {
            if (chk > 1)
            {
                U = I;
            }
            else
            {
                U = (Matrix)(A * V);
                for (int l = 0; l < 3; l++)
                {
                    if (l != pos)
                    {
                        for (int m = 0; m < 3; m++)
                        {
                            U[m, l] *= 1 / sigma[l];
                        }
                    }
                }

                Vector[] v = new Vector[2];
                int index = 0;
                for (int l = 0; l < 3; l++)
                {
                    if (l != pos)
                    {
                        v[index++] = Vector.OfArray([U[0, l], U[1, l], U[2, l]]);
                    }
                }
                Vector vec = v[0].Cross(v[1]);
                vec = (Vector)vec.Normalize(2);
                U[0, pos] = vec[0];
                U[1, pos] = vec[1];
                U[2, pos] = vec[2];
            }
        }
        else
        {
            Vector sigmaInv = Vector.OfArray([1.0 / sigma[0], 1.0 / sigma[1], 1.0 / sigma[2]]);
            U = (Matrix)(A * V);
            for (int l = 0; l < 3; l++)
            {
                for (int m = 0; m < 3; m++)
                {
                    U[m, l] *= sigmaInv[l];
                }
            }
        }

        double detU = U.Determinant();

        // U is a reflection => inversion
        if (detU < 0.0)
        {
            //std::cout << "Inversion!\n";
            double minLambda = double.MaxValue;
            pos = 0;
            for (int l = 0; l < 3; l++)
            {
                if (sigma[l] < minLambda)
                {
                    pos = l;
                    minLambda = sigma[l];
                }
            }

            // invert values of smallest singular value
            sigma[pos] = -sigma[pos];
            U[0, pos] = -U[0, pos];
            U[1, pos] = -U[1, pos];
            U[2, pos] = -U[2, pos];
        }
        return (sigma, U, VT);
    }
}
